#include "PhysicalObject.h"


PhysicalObject::PhysicalObject(){
	//physics
	this->mass=1;
	this->position = Vector3f();
	this->centreOfGravity = Vector3f();
	this->velocity = Vector3f();
	this->bbox = new Vector3f[2];
	this->createDefaultBbox();
	grav = false;
	//rendering
	myTBatch = new GLTriangleBatch();
	gltMakeSphere(*myTBatch,0.1f,16,16);

	myShader = gltLoadShaderPairWithAttributes(
		"phong.vert", "phong.frag", 3,
		GLT_ATTRIBUTE_VERTEX, "vVertex",
		GLT_ATTRIBUTE_NORMAL, "vNormal",
		GLT_ATTRIBUTE_TEXTURE0, "vTexture0");

	locAmbient = glGetUniformLocation(myShader, "ambientColor");
	locDiffuse = glGetUniformLocation(myShader, "diffuseColor");
	locSpecular = glGetUniformLocation(myShader, "specularColor");
	locLight = glGetUniformLocation(myShader, "vLightPosition");
	locMVP = glGetUniformLocation(myShader, "mvpMatrix");
	locMV  = glGetUniformLocation(myShader, "mvMatrix");
	locNM  = glGetUniformLocation(myShader, "normalMatrix");
	locTexture = glGetUniformLocation(myShader, "colorMap");

}
void PhysicalObject::createDefaultBbox(){
	bbox[0].x = bbox[0].y = bbox[0].z = -0.5f;
	bbox[1].x = bbox[1].y = bbox[1].z = 0.5f;
}

PhysicalObject::PhysicalObject(	float m,
								Vector3f p,
								Vector3f cog,
								Vector3f v,
								char* fname,
								bool multi){
	this->mass = m;
	this->position = p;
	this->centreOfGravity = cog;
	this->velocity = v;
	this->bbox = new Vector3f[2];
	this->createDefaultBbox();
	this->multipart = multi;
	myFrame.SetOrigin(p.x,p.y,p.z);
	//myFrame.RotateWorld(PI/2,0.0f,1.0f,0.0f);
	grav = false;
	//rendering
	//gltMakeSphere(myTBatch,0.1f,16,16);
	min_x = 0;
	max_x = 0;
	min_y = 0;
	max_y = 0;
	min_z = 0;
	max_z = 0;

	if(multipart){
		numParts = loaders::uwsmMultiCheck(fname);
		myTBatch = new GLTriangleBatch[numParts];
		loaders::uwsmMultiLoad(fname,myTBatch,min_x, max_x, min_y, max_y, min_z, max_z);
	}else{
		myTBatch = new GLTriangleBatch();
		loaders::uwsm(fname,myTBatch,min_x, max_x, min_y, max_y, min_z, max_z);
	}

	//myFrame.RotateLocalY(PI/2);

	myShader = gltLoadShaderPairWithAttributes(
		"phong.vert", "phong.frag", 3,
		GLT_ATTRIBUTE_VERTEX, "vVertex",
		GLT_ATTRIBUTE_NORMAL, "vNormal",
 		GLT_ATTRIBUTE_TEXTURE0, "vTexture0");

	locAmbient = glGetUniformLocation(myShader, "ambientColor");
	locDiffuse = glGetUniformLocation(myShader, "diffuseColor");
	locSpecular = glGetUniformLocation(myShader, "specularColor");
	locLight = glGetUniformLocation(myShader, "vLightPosition");
	locMVP = glGetUniformLocation(myShader, "mvpMatrix");
	locMV  = glGetUniformLocation(myShader, "mvMatrix");
	locNM  = glGetUniformLocation(myShader, "normalMatrix");
	locTexture = glGetUniformLocation(myShader, "colorMap");

	width = max_x - min_x;
	height = max_y - min_y;
	depth = max_z - min_z;
}

void PhysicalObject::setMass(float newMass){this->mass = newMass;}
float PhysicalObject::getMass(){return this->mass;}

Vector3f PhysicalObject::getPos(){return this->position;}
void PhysicalObject::setPos(Vector3f newPos){this->position = newPos;}
void PhysicalObject::setPos(float x, float y, float z){this->position = Vector3f(x,y,z);}

Vector3f PhysicalObject::getCOG(){return this->centreOfGravity;}
void PhysicalObject::setCOG(Vector3f newCOG){this->centreOfGravity = newCOG;}
void PhysicalObject::setCOG(float x, float y, float z){this->centreOfGravity = Vector3f(x,y,z);}

Vector3f PhysicalObject::getVel(){return this->velocity;}
void PhysicalObject::setVel(Vector3f newVel){this->velocity = newVel;}
void PhysicalObject::setVel(float x, float y, float z){this->velocity = Vector3f(x,y,z);}

//might need to convert to world coords down the line
void PhysicalObject::getBbox(Vector3f* pbbox){
	pbbox[0] = bbox[0];
	pbbox[1] = bbox[1];
}
void PhysicalObject::setBbox(Vector3f* newBbox){
	this->bbox[0] = newBbox[0];
	this->bbox[1] = newBbox[1];
}

void PhysicalObject::addForce(Vector3f force)
{
	totalForce += force;
}

void PhysicalObject::exchangeMomentum(PhysicalObject* otherObject)
{
	// momentum = mass * velocity
	// (m1 + m2) = (m1 + m2) after
	// C is the coefficient of restitution a float that describes the elaticity of the collision.
	float C = 0.01;  // C is set to simulate a coolision with loss of momentum.

	// need to calculate the results for the exchange of velocities in each direction seperately.
	Vector3f temp,ub;
	float mb = otherObject->getMass();// mb =1;  // this had been set as 1 for some reason.
	ub = otherObject->getVel();
	float ma = mass;
	Vector3f ua = velocity;
	
	// x-axis

	

	velocity.x =	(ua.x*(ma-(C*mb))+ub.x*mb*(1+C))/(ma+mb);
	temp.x =		((ua.x*ma*(1+C))+(ub.x*(mb-(C*ma))))/(ma+mb);
	velocity.y =	(ua.y*(ma-(C*mb))+ub.y*mb*(1+C))/(ma+mb);
	temp.y =		((ua.y*ma*(1+C))+(ub.y*(mb-(C*ma))))/(ma+mb);
	velocity.z =	(ua.z*(ma-(C*mb))+ub.z*mb*(1+C))/(ma+mb);
	temp.z =		((ua.z*ma*(1+C))+(ub.z*(mb-(C*ma))))/(ma+mb);


	otherObject->setVel(temp);
	cDump();
}

void PhysicalObject::update(float dt)
{
	//  gives the bullet the appearence of spinning.  as the inerited XTarget class has its own update which overwrites this
											// this is ok, also the only other Physical object (the target frame) has 0 velocity so doesn't rotate.			
	
	Vector3f temp,a,tvel;
	a = totalForce/mass;
	velocity += a;   
	tvel = velocity;
	tvel *= dt;
	temp = position + (tvel * dt);
	if(temp.y < -50)
	{
		temp.y = -50;
		velocity *= 0.6f;
		velocity.y *= -1;
	}
	if(temp.x < -90 && temp.y < 18){
		temp.x = -90;
		velocity *= 0.1f;
		velocity.x *= -1;
	}
	if(temp.x > 90 && temp.y < 18){
		temp.x = 90;
		velocity *= 0.1f;
		velocity.x *= -1;
	}

	myFrame.SetOrigin(temp.x, temp.y,temp.z);
	position = temp;
	//clear forces
	totalForce.clear();
	myFrame.RotateLocalZ((tvel.z*0.01)*dt);	
	if(grav){
		addForce(GVEC * dt);	
	}
	std::cout << position.x << ".." << position.y << ".." << position.z << std::endl;
}

void PhysicalObject::Render(
		GLMatrixStack *pMVM,
		GLGeometryTransform *pGLGT,
		GLuint *texture,
		Camera *camIn){
	
	pMVM->PushMatrix();
	pMVM->MultMatrix(this->myFrame); 
	//if(multipart){
	M3DMatrix44f tmx;
	
	m3dRotationMatrix44(tmx,PI/2,0.0f,1.0f,0.0f);
	//pMVM->MultMatrix(tmx);
	//pMVM->Scale(0.01f,0.01f,0.01f);
	//}
	M3DVector4f vEyeLight,vLight = { 1694, 1044, -2061 ,1.0f};
	M3DMatrix44f *mx;
	mx = camIn->getMx();
    m3dTransformVector4(vEyeLight, vLight, *mx);
		
	GLfloat vAmbientColor[] = { 0.4f, 0.4f, 0.4f, 1.0f };
	GLfloat vDiffuseColor[] = { 1.0f, 1.0f, 1.0f, 1.0f};
	GLfloat vSpecularColor[] = { 1.0f, 1.0f, 1.0f, 1.0f };

	glBindTexture(GL_TEXTURE_2D,texture[0]);
	glUseProgram(myShader);
	glUniform4fv(locAmbient, 1, vAmbientColor);
	glUniform4fv(locDiffuse, 1, vDiffuseColor);
	glUniform4fv(locSpecular, 1, vSpecularColor);
	glUniform3fv(locLight, 1, vEyeLight);
	glUniformMatrix4fv(locMVP, 1, GL_FALSE, pGLGT->GetModelViewProjectionMatrix());
	glUniformMatrix4fv(locMV, 1, GL_FALSE, pGLGT->GetModelViewMatrix());
	glUniformMatrix3fv(locNM, 1, GL_FALSE, pGLGT->GetNormalMatrix());
	glUniform1i(locTexture, 0);

	if(multipart){
		
		for(int i = 0;i<numParts;i++){
			myTBatch[i].Draw();
		}
	}else{
		myTBatch->Draw();
	}

	pMVM->PopMatrix();
}
void PhysicalObject::cDump(){
//	system("cls");
	std::cout	<< "Mass                  = (" << mass << ")" << std::endl
				<< "Position(WS)(GLunits) = (" << position.x << ", " << position.y << ", " << position.z << ")" << std::endl
				<< "Velocity(GLunits/dt)  = (" << velocity.x << ", " << velocity.y << ", " << velocity.z << ")" << std::endl
				<< "COG(Local Space)      = (" << centreOfGravity.x << ", " << centreOfGravity.y << ", " << centreOfGravity.z << ")" << std::endl;
}


// This whole function could be simplified by creating a function in the Vector class to find the intersection of two lines
// I will work on that at some point today or tommorrow.  Advantage of this is it will also automatically check the x-axis.
bool PhysicalObject::isTouching(PhysicalObject* otherObject, float dt)
{
	bool temp= false;
	if(position.x < otherObject->getWidth()/2 && position.x > (0 - otherObject->getWidth() / 2))
	{
		if(position.z < otherObject->getPos().z)
		{
			std::cout << " past target" << std::endl;
		}
		// two points that together define a line detailing the trajectory in 2 dimensions of the object over the last update
		float z1 = position.z;								// z-position now
		float y1 = position.y;								// y-position now

		float z2 = (position.z - (velocity.z *dt));			// z-position previous to last update
		float y2 = (position.y - (velocity.y * dt));		// y-position previous to last update


		// z = mx + c
		float m =(y1 - y2) / (z1 - z2);
		float c = -1 * ((m * z2) - y2);
		// two points that together define a line detailing the face of the target in 2-dimensions.  As the target is fixed in the x_axis
		// the x can be ignored
		float oy1 = 0;
		float oy2 = 0;
		float oz1 = 0;
		float oz2 = 0;
		if(otherObject->getRotation() == 0)
		{
			oy1 = (otherObject->getPos().y - otherObject->getHeight()/2);
			oy2 = (otherObject->getPos().y + otherObject->getHeight()/2);
			oz1 = otherObject->getPos().z;
			oz2 = otherObject->getPos().z;
		}
		else 
		{
			oz1 = otherObject->getPos().z - sin(otherObject->getRotation() * DegToRad) * (otherObject->getHeight() / 2);
			oy1 = otherObject->getPos().y - cos(otherObject->getRotation() * DegToRad) * (otherObject->getHeight() / 2);
		
			oz2 = otherObject->getPos().z + sin(otherObject->getRotation() * DegToRad) * (otherObject->getHeight() / 2);
			oy2 = otherObject->getPos().y + cos(otherObject->getRotation() * DegToRad) * (otherObject->getHeight() / 2);	
		}
		Vector3f point1 = Vector3f(position.x,position.y,position.z);
		Vector3f point2 = Vector3f(position.x - velocity.x * dt,position.y - velocity.y * dt,position.z - velocity.z * dt);
		Vector3f point3 = Vector3f(otherObject->getPos().x - (otherObject->getWidth() / 2), oy1, oz1);
		Vector3f point4 = Vector3f(otherObject->getPos().x + (otherObject->getWidth() / 2), oy2, oz2);
		float om = (oy1 - oy2) / (oz1 - oz2);
		float oc = -1 * ((om * oz2) - oy2);
		float y_at_contact = 0;
		float z_at_contact = 0;
		if(oz1 == oz2)
		{
			y_at_contact = m * oz1 + c;
			z_at_contact = oz1; 
		}
		else
		{
			z_at_contact = (c - oc) / (om - m);
			y_at_contact = (om * z_at_contact) + oc;
		}
	
		// check if collision occurs within the defined points on the line describing this objects change in position
  		if(z_at_contact > z1 && z_at_contact < z2)
		{
			if(oy1 > 0)
			{
				oy1 *= -1;
				oy2 *= -1;
			}
 			//then check if the collision occurs within the defined ends of the line describing the targets edge
			if(y_at_contact > oy1 && y_at_contact < oy2)
			{
 				exchangeMomentum(otherObject);
 				otherObject->getPointOfImpact(z_at_contact, y_at_contact);
				position.z = z_at_contact + depth;
				position.y = y_at_contact;
				temp = true;
			}
		}
	}
	return temp;
	////alternative
	//float u = 0;  // intersec
	//if (position.x > (otherObject->getPos().y - (otherObject->getWidth() / 2)) && position.x < (otherObject->getPos().y + (otherObject->getWidth() / 2)))
	//{
	//	Vector3f temp = position - otherObject->getPos();
	//	if(temp.length() <= otherObject->getHeight() / 2)
	//	{
	//		u = (otherObject->getNormal().dotProduct(otherObject->getNormalPoint() - position)) / (otherObject->getNormal().dotProduct(position - (position - velocity)));
	//		if(u < 0 && u > 1)
	//		{
	//			exchangeMomentum();
	//		}
	//	}
	//}

	//float d = 0; // intersect
}

void PhysicalObject::setForwardVector(Vector3f v){
	//if v is a zero vector ([0,0,0]), dont bother trying to do anything.
	if(!v.isZero())
	{
		Vector3f u,f,r;
		M3DVector3f up,fwd,rt;
		myFrame.GetForwardVector(fwd);
		myFrame.GetUpVector(up);
				
		u.fromM3D(up);
		f.fromM3D(fwd);
		//right handed
		r = f.crossProduct(u);
		r.normalizeMe();
		//okay...
		f = v;
		f.normalizeMe();
		
		u = r.crossProduct(f);
		u.normalizeMe();
		f.toM3D(fwd);
		myFrame.SetForwardVector(fwd);
		u.toM3D(up);
		myFrame.SetUpVector(up);

	}
}

Vector3f PhysicalObject::getForwardVector(){
	
	Vector3f t;
	t.x = velocity.x;
	t.y = velocity.y;
	t.z = velocity.z;

	return t;
}